#ifndef _DEFINITIONS_H_
#define _DEFINITIONS_H_

#include <moying_costmap/moying_costmap.h>
#include <moying_proto/moying_proto.h>
#include <schedule_config/config.hpp>
#include <map>
#include <vector>
#include <ctime>
#include <log4cplus/logger.h>
#include <log4cplus/loggingmacros.h>
#include <log4cplus/configurator.h>
#include <log4cplus/initializer.h>

#define __DEBUG__ 1

namespace moying
{
namespace navigation
{

    class MoyingMap;
    typedef std::shared_ptr<MoyingMap> MoyingMapPtr;
    class CustomMovePatterns;
    typedef std::shared_ptr<CustomMovePatterns> CustomMovePatternsPtr;

    struct VehicleState
    {
        double x;       //世界坐标系x坐标
        double y;       //世界坐标系y坐标
        double th;      //世界坐标系朝向
        double vel_x;   //x轴方向速度
        double vel_y;   //y轴方向速度 
        double vel_th;  //旋转速度 
        std::chrono::system_clock::time_point t;    //机器人到达该点的时刻
        // bool replan = false;
        int move_mode;  //0:前进,1:原地旋转,2:左横向移动,3:右横向移动,4:后退
        VehicleState():x(0),y(0),th(0),vel_x(0),vel_y(0),vel_th(0){};
    };

    enum MovePatternState{
        kStaticState = 0,           //静止
        // kRotateState,            //旋转
        kMoveForwardState,          //向前移动
        kLeftMoveShiftState,        //左横向移动
        kRightMoveShiftState,       //右横向移动
        kMoveBackwardState,         //后退
    };

    enum MovePatternType{
        kAcceleratePattern  = 0,    //加速
        // kMoveForwardStopPattern,    //静止状态下前进一格并停止
        kReduceSpeedPattern,        //减速,停在下一格
        kWaitPattern,               //原地等待
        kRotateLeft45Pattern,       //左转45
        kRotateLeft90Pattern,       //左转90
        kRotateLeft135Pattern,      //左转135
        kRotate180Pattern,          //旋转180
        kRotateR45Pattern,          //右转45
        kRotateR90Pattern,          //右转90
        kRotateR135Pattern,         //右转135
        kMoveForwardPattern,        //匀速前进
        kLeftTranslationPattern,    //左平移
        kRightTranslationPattern,   //右平移
        kLeftFrontPattern,          //左前方
        kRightFrontPattern,         //右前方
        kLeftTransAccePattern,      //左平移加速
        kRightTransAccePattern,     //右平移加速
        kLeftTransReducePattern,    //左平移减速
        kRightTransReducePattern,   //右平移减速
        kTurnLeftPattern,           //左转弯
        kTurnRightPattern,          //右转弯
        kAccelerateBackPattern,     //加速后退
        kReduceSpeedBackPattern,    //减速后退
        kMoveBackwardPattern,       //匀速后退

    };

    //地图上的朝向
    enum Orientation{
        kOrientRight    = 0,    //theta=0
        kOrientRightTop,        //theta=1.57/2
        kOrientTop ,            //theta=1.57
        kOrientLeftTop ,        //theta=1.57*3/2
        kOrientLeft ,           //theta=3.14
        kOrientLeftBottom ,     //theta=-1.57*3/2
        kOrientBottom ,         //theta=-1.57
        kOrientRightBottom ,    //theta=-1.57/2
        kUndefine,              //未确定 
    };

    struct MovePattern
    {
        MovePattern(int _x, int _y, int _dir_change, int _t, int _cost, MovePatternState _sm_state)
        :x(_x), y(_y), dir_change(_dir_change), t(_t), cost(_cost), sm_state(_sm_state)
        {

        }
        int x;
        int y;
        int dir_change;
        int t;
        int cost;
        MovePatternState sm_state;
    };

    //路径规划参数
    struct PathPlanParam
    {
        float nav_speed;                //导航速度
        float map_plan_resolution;      //地图路径规划分辨率
        float time_map_resolution;      //时间地图分辨率
    };

    // typedef std::vector<MovePattern> MovePatterns;
    typedef std::vector<MovePatternType> MovePatterns;  
    typedef std::shared_ptr<MovePatterns> MovePatternsPtr;

    struct NonTimedNode;
    typedef std::shared_ptr<NonTimedNode> NonTimedNodePtr;
    
    struct Node;
    typedef Node* NodePtr;
    struct Node
    {
        Node(){}
        Node(int _x, int _y, int _th, int _t)
        :x(_x), y(_y), th(_th), t(_t),theta(0)
        {
        }
        Node(int _x, int _y, int _th, int _t, int _sm_state)
        :x(_x), y(_y), th(_th), t(_t) , sm_state(_sm_state),theta(0)
        {
        }

        NodePtr parent; //父节点指针
        short x;        //路径规划地图坐标系x坐标
        short y;        //路径规划地图坐标系y坐标
        short t;        //机器人走到路径中该节点的时刻,单位为时间地图分辨率
        
        short f;        //该节点在A*算法中的总代价
        short g;        //该节点在A*算法中的已有代价
        char sm_state;  //运动状态机,0:静止,1:匀速运动.2:平移
        char th;        //机器人朝向标志
        float theta;    //如果该值不为0,用该值做为实际的朝向

        bool isEqual(const Node& node,bool without_time=true){
            bool r = node.x == x && node.y == y && node.th == th && node.sm_state == sm_state;
            if(!without_time)
                r = r && node.t == t;
            return r;
        }

    };

    struct MapDimension{
        int x;
        int y;
        int theta;
        int time;
        int sm_state;
    };

    struct Coordinate{
        double x;
        double y;
        Coordinate(){};
        Coordinate(double x1,double y1){x=x1;y=y1;}
    };

    struct CoordinateInt{
        int x;
        int y;
        CoordinateInt(){};
        CoordinateInt(int x1,int y1){x=x1;y=y1;}
        // void operator=(const CoordinateInt& c){x=c.x;y=c.y;}
    };

    //更正过的cell信息
    struct CorrectCellInfo{
        int cell_x;         //cell的x坐标  
        int cell_y;         //cell的y坐标
        int raw_map_x;      //原始地图x坐标
        int raw_map_y;      //原始地图y坐标
        float obs_factor;   //周围障碍物因子,障碍物越多,因子数值越大
        short theta;        //机器人位于该cell时可以允许的朝向,每一位代表一个方向,1表示允许,0不允许
        unsigned char cost; //当前cell静态地图中的代价值
        bool can_rotate;    //当前cell是否可旋转
        CorrectCellInfo():cell_x(-1),cell_y(-1),raw_map_x(-1),raw_map_y(-1),theta(-1),cost(-1),can_rotate(true){}
    };

    //路径搜索时的边界限制
    struct SearchCellArea
    {
        bool valid=false;     //该边界限制是否有效
        int min_cell_x; //x维度最小坐标,路径规划地图分辨率下
        int min_cell_y; //y维度最小坐标
        int max_cell_x; //x维度最大坐标
        int max_cell_y; //y维度最大坐标
    };

    struct MapCell
    {
        int x;
        int y;
        MapCell(){};
        MapCell(int _x,int _y):x(_x),y(_y){}
    };

    struct FootprintCell
    {
        FootprintCell()
        {
            
        }
        FootprintCell(int _x, int _y, bool _inflate)
        :x(_x), y(_y), inflate(_inflate)
        {

        }
        int x;
        int y;
        bool inflate;
    };

    typedef std::map<int, FootprintCell> FootprintOnMap;  //栅格地图上的足迹容器,key:栅格地图的栅格索引,value:该栅格点对应的足迹信息

    using TimedFootprints = std::map<int, std::vector<FootprintCell>>; //key:时刻,value:costmap中的占用足迹
    struct PlanningResult
    {
        std::vector<VehicleState> path;
        std::vector<Node> node_path;
        TimedFootprints timed_footprints; //路径在时间地图中的足迹
        VehicleState start;
        VehicleState goal;
        std::chrono::system_clock::time_point start_time;
        std::chrono::system_clock::time_point goal_time;
        unsigned int occupy_time_after_arrive=0;  //到达目标点后持续占用时间地的秒数
        moying_proto::NavType         nav_type = moying_proto::NavType::kNavTypeTrafficManager;   //导航方式
        moying_proto::ConstantPathInfo    cpi;    //固定路径信息,只有导航方式是固定路径导航时才有用

        void clear(){
            path.clear();
            node_path.clear();
            timed_footprints.clear();
            cpi.clear_path_point();
            cpi.set_path_id("");
        }
    };

    struct ColorRGBA
    {
        ColorRGBA()
        :r(1), g(0), b(0), a(0.1)
        {

        }
        double r;
        double g;
        double b;
        double a;
    };


    //局部障碍物信息
    struct LocalObstacleInfo{
        moying_proto::LocalObstacle raw_obstacle;     //原始地图分辨率下的障碍物
        moying_proto::LocalObstacle temp_obstacle;    //除地图外的临时障碍物
        std::vector<std::pair<int,int>> occupancy_cells;             //占用的规划地图格子索引
        int min_cell_x;
        int min_cell_y;
        int max_cell_x;
        int max_cell_y;
        float   min_obstacle_dis;       //机器人边缘离障碍最近的距离,单位米
        std::chrono::system_clock::time_point last_recv_time;   //上一次收到局部障碍物的时间
    };


    #define SAME_POINT_THETA_DIS  0.001        //差小于该值的两个theta认为是同一个
    #define SAME_POINT_COOR_DIS      0.01        //距离小于该值的坐标认为是同一个

    //判断两个坐标是否相等
    #define SAME_POSE(state1,state2) (fabs(state1.x-state2.x) < SAME_POINT_COOR_DIS && \
                fabs(state1.y-state2.y) < SAME_POINT_COOR_DIS && fabs(state1.th-state2.th) < SAME_POINT_THETA_DIS)

    //判断两个浮点数是否相等
    #define EQUAL_FLOAT(d1,d2) (fabs( (d1)-(d2) ) < 0.001)



    //日志器
    extern log4cplus::Logger logger;

    #define LOG_ERROR(msg) LOG4CPLUS_ERROR(logger, LOG4CPLUS_TEXT(msg));
    #define LOG_WARN(msg) LOG4CPLUS_WARN(logger, LOG4CPLUS_TEXT(msg));
    #define LOG_DEBUG(msg) LOG4CPLUS_DEBUG(logger, LOG4CPLUS_TEXT(msg));
    #define LOG_INFO(msg) LOG4CPLUS_INFO(logger, LOG4CPLUS_TEXT(msg));
    #define LOG_TRACE(msg) LOG4CPLUS_TRACE(logger, LOG4CPLUS_TEXT(msg));
    #define LOG_CONSOLE(msg) printf("%s\n",msg);
}
}

#endif // _DEFINITIONS_H_